﻿using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Interop;

namespace DubaiCoffee.Model
{
    /// <summary>
    /// 机器人控制
    /// </summary>
    public class RobotOperate
    {
        public static string RobotOperateUrl = AppDomain.CurrentDomain.SetupInformation.ApplicationBase;
        public static int handle1 = 0;
        public static int handle2 = 0;
        public static double PI = 3.1415926535;
        public static List<string[]> listStrArr1 = new List<string[]>();//数组List，相当于可以无限扩大的二维数组。
        public static List<string[]> listStrArr2 = new List<string[]>();
        public static JKTYPE.ProgramState pstatus;
        private static RobotOperate instance;
        private RobotOperate() { }
        public static RobotOperate Instance
        {
            get
            {
                if (instance == null)
                {
                    instance = new RobotOperate();
                }
                return instance;
            }
        }
        public static void CX_Start()
        {
            try
            {
                jakaAPI.get_program_state(ref handle1, ref pstatus);
            }
            catch (Exception)
            {
                return;
            }
        }
        public  JKTYPE.ProgramState pstatusL;
        public  JKTYPE.RobotState stateL;
        public  JKTYPE.RobotStatus statusL;
        public  void CX_Start_L()
        {
            try
            {
                jakaAPI.get_program_state(ref handle1, ref pstatusL);
            }
            catch (Exception)
            {
                return;
            }
        }
        public  void CX_kj_L()
        {
            try
            {
                jakaAPI.get_robot_state(ref handle1, ref stateL);
            }
            catch (Exception)
            {
                return;
            }
        }
        public  void CX_status_L()
        {
            try
            {
                jakaAPI.get_robot_status(ref handle1, ref statusL);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static  void Robot_Initialisation_double()//机器人初始化
        {
            IOMsg.Instance.AllBol = false;
            Console.WriteLine(DigitalMsg.Instance.RobotPowerOn_start);
            Thread.Sleep(200);
            IOSerialPort.Instance.SerialSend(DigitalMsg.Instance.RobotPowerOn_start);
            Thread.Sleep(500);
            IOMsg.Instance.AllBol = true;
            int tool_idset1 = 0;
            int frame_idset1 = 0;
            int result1 = -1;
            while (result1 == -1)
            {
                result1 = jakaAPI.create_handler(IPorPortInfo.Instance.RobotIPLeft.ToCharArray(), ref handle1);
                Thread.Sleep(100);
            }
            IOMsg.Instance.AllBol = false;
            Thread.Sleep(100);
            IOSerialPort.Instance.SerialSend(DigitalMsg.Instance.RobotPowerOn_end);
            Thread.Sleep(100);
            IOMsg.Instance.AllBol = true;
            System.Threading.Thread.Sleep(3000); //毫秒
            //机器人上电
            jakaAPI.power_on(ref handle1); Console.WriteLine("L_Robot Power On");
            //切换当前使用的工具坐标
            jakaAPI.set_tool_id(ref handle1, tool_idset1); Console.WriteLine("L_Robot_T " + tool_idset1);
            //切换当前使用的世界坐标
            jakaAPI.set_user_frame_id(ref handle1, frame_idset1); Console.WriteLine("L_Robot_F " + frame_idset1);
            StringBuilder Version = new StringBuilder("", 3000);
            int result3 = jakaAPI.get_sdk_version(ref handle1, Version);
            Console.WriteLine("SDK 版本 " + Version);
            IOMsg.Instance.QK();
            Thread.Sleep(3000);
            ClientList clientList1 = new ClientList();
            clientList1.MsgBol = WuLiaoCoffeeSel.Instance;
            clientList1.code = 12;
            clientList1.IsMake = false;
            SocketClients.Instance.client.Send(Encoding.UTF8.GetBytes(JsonConvert.SerializeObject(clientList1)));
        }
        public void Robot_L_Show()
        {
            CX_status_L();
            CX_Start_L();
            if (statusL.powered_on == 1)
                RobotMsgs(false, true, false, false, false, false, false);
            if (statusL.enabled == 1)
                RobotMsgs(false, false, true, false, false, false, false);
            if (pstatusL == JKTYPE.ProgramState.PROGRAM_RUNNING)
            {
                RobotMsgs(false, false, false, true, false, false, false);
                if (WuLiaoCoffeeSel.Instance.RobotWarn != true)
                {
                    WuLiaoCoffeeSel.Instance.RobotWarn = true;
                    ClientList clientList2 = new ClientList();
                    clientList2.MsgBol = WuLiaoCoffeeSel.Instance;
                    clientList2.code = 2;
                    SocketClients.Instance.client.Send(Encoding.UTF8.GetBytes(JsonConvert.SerializeObject(clientList2)));
                    Thread.Sleep(1000);
                }
            }
            if (pstatusL == JKTYPE.ProgramState.PROGRAM_IDLE)
                RobotMsgs(false, false, false, false, true, false, false);
            if (pstatusL == JKTYPE.ProgramState.PROGRAM_PAUSED)
                RobotMsgs(false, false, false, false, false, true, false);
            if (statusL.enabled == 0)
                RobotMsgs(false, true, false, false, false, false, false);
            if (statusL.powered_on == 0)
                RobotMsgs(true, false, false, false, false, false, false);
            if (statusL.errcode != 0)
                RobotMsgs(false, false, false, false, false, false, true);
        }

        /// <summary>
        /// 机器人状态
        /// </summary>
        /// <param name="KJ">开机</param>
        /// <param name="SD">上电</param>
        /// <param name="SN">使能</param>
        /// <param name="YX">运行</param>
        /// <param name="TZ">停止</param>
        /// <param name="ZT">暂停</param>
        /// <param name="BJ">报警</param>
        ///  <param name="type">左右手 0 左 1 右</param>
        public void RobotMsgs(bool KJ, bool SD, bool SN, bool YX, bool TZ, bool ZT, bool BJ, int type = 0)
        {
            if (type == 0)
            {
                RobotMsg.Instance.Robot_YKJ_L = KJ;
                RobotMsg.Instance.Robot_SD_L = SD;
                RobotMsg.Instance.Robot_SN_L = SN;
                RobotMsg.Instance.Robot_YX_L = YX;
                RobotMsg.Instance.Robot_TZ_L = TZ;
                RobotMsg.Instance.Robot_ZT_L = ZT;
                RobotMsg.Instance.Robot_BJ_L = BJ;
            }
            else
            {
                //RobotMsg.Instance.Robot_YKJ_R = KJ;
                //RobotMsg.Instance.Robot_SD_R = SD;
                //RobotMsg.Instance.Robot_SN_R = SN;
                //RobotMsg.Instance.Robot_YX_R = YX;
                //RobotMsg.Instance.Robot_TZ_R = TZ;
                //RobotMsg.Instance.Robot_ZT_R = ZT;
                //RobotMsg.Instance.Robot_BJ_R = BJ;
            }
        }
        public void Robot_Initial_Point()//通过关节直线到初始点
        {
            //定义并初始化关JointValue变量
            JKTYPE.JointValue joint_pos1 = new JKTYPE.JointValue();
            JKTYPE.JointValue joint_pos11 = new JKTYPE.JointValue();
            JKTYPE.JointValue joint_pos2 = new JKTYPE.JointValue();
            JKTYPE.JointValue joint_pos22 = new JKTYPE.JointValue();
            //yjx_1J_412 yjx_2J_412
            joint_pos11.jVal = new double[] { 262.227 / 57.3, -43.297 / 57.3, -104.307 / 57.3, 121.504 / 57.3, -73.785 / 57.3, 117.196 / 57.3 };//初始
            joint_pos1.jVal = new double[] { 7.160 / 57.3, 27.992 / 57.3, 119.469 / 57.3, -142.118 / 57.3, 62.838 / 57.3, 159.148 / 57.3 };
            joint_pos2.jVal = new double[] { 292.580 / 57.3, -15.393 / 57.3, -122.816 / 57.3, 110.422 / 57.3, -66.025 / 57.3, 136.822 / 57.3 };
            joint_pos22.jVal = new double[] { 48.915 / 57.3, 29.870 / 57.3, 117.332 / 57.3, -161.063 / 57.3, 58.664 / 57.3, 169.889 / 57.3 };
            Console.WriteLine("joint_move_开始");
            //阻塞运动到目标点位
            jakaAPI.joint_move(ref handle1, ref joint_pos22, JKTYPE.MoveMode.ABS, true, 1);
            jakaAPI.joint_move(ref handle1, ref joint_pos1, JKTYPE.MoveMode.ABS, true, 1);
            jakaAPI.joint_move(ref handle2, ref joint_pos11, JKTYPE.MoveMode.ABS, true, 1);
            jakaAPI.joint_move(ref handle2, ref joint_pos2, JKTYPE.MoveMode.ABS, true, 1);
            //System.Threading.Thread.Sleep(1500); //毫秒
            Console.WriteLine("joint_move_结束");
            //linear_move指令执行开始、
            Console.WriteLine("linear_move_开始");
            JKTYPE.CartesianPose tcp_pos1 = new JKTYPE.CartesianPose();
            JKTYPE.CartesianPose tcp_pos2 = new JKTYPE.CartesianPose();
            //yjx_1J_412   yjx_2J_412
            tcp_pos1.tran.x = 37.274; tcp_pos1.tran.y = 352.718; tcp_pos1.tran.z = 192.557;
            tcp_pos1.rpy.rx = 104.960 / 57.3; tcp_pos1.rpy.ry = -88.416 / 57.3; tcp_pos1.rpy.rz = 139.090 / 57.3;
            tcp_pos2.tran.x = -205.981; tcp_pos2.tran.y = -226.677; tcp_pos2.tran.z = 217.933;
            tcp_pos2.rpy.rx = -95.242 / 57.3; tcp_pos2.rpy.ry = 7.952 / 57.3; tcp_pos2.rpy.rz = 171.149 / 57.3;

            jakaAPI.linear_move(ref handle1, ref tcp_pos1, JKTYPE.MoveMode.ABS, true, 30);
            jakaAPI.linear_move(ref handle2, ref tcp_pos2, JKTYPE.MoveMode.ABS, true, 30);
            Console.WriteLine("linear_move_结束");
        }
        public void Robot_Yun()
        {
            try
            {
                jakaAPI.program_run(ref handle1);
            }
            catch (Exception)
            {
                return;
            }
        }
    }
}
